#include <rclcpp/rclcpp.hpp>
#include <sensor_msgs/msg/laser_scan.hpp>
#include "std_msgs/msg/float32_multi_array.hpp"

float PI=3.141593;
class ObsDet: public rclcpp::Node
{
    private:
        // 声明话题发布者
        rclcpp::Publisher<std_msgs::msg::Float32MultiArray>::SharedPtr command_publisher_;
        //声明雷达主题
        rclcpp::Subscription<sensor_msgs::msg::LaserScan>::SharedPtr laser_scan_sub_;
        
        void lidar_callback(const sensor_msgs::msg::LaserScan::SharedPtr msg)
        {
            // 创建消息
            std_msgs::msg::Float32MultiArray lidar_value;
            // RCLCPP_INFO(this->get_logger(),"雷达已接收");
            lidar_value.data.resize(2);
            float shortest_distance = std::numeric_limits<float>::max();
            float shortest_angle = 0.0;
            // 遍历激光雷达数据，找到最短距离
            for (size_t i = 0; i < msg->ranges.size(); i++)
            {
                if (msg->ranges[i]<shortest_distance && msg->ranges[i]>0.001)
                {
                    shortest_distance=msg->ranges[i];
                    shortest_angle=msg->angle_min+msg->angle_increment*i;
                }
            }
            //导入至lidar_value
            if (shortest_distance>4.0)
            {
                shortest_distance=4.0;
                shortest_angle=0.0;
            }
            else if (shortest_distance<msg->range_min)
            {
                shortest_distance=msg->range_min;
            }
            
            // if (shortest_angle>PI/2.0)
            // {
            //     shortest_angle=PI/2.0;
            // }
            // else if (shortest_angle<-PI/2.0)
            // {
            //    shortest_angle=-PI/2.0;
            // }
            // RCLCPP_INFO(this->get_logger(),"最短距离=%f,对应角度=%f",shortest_distance,shortest_angle);
            lidar_value.data[0]=shortest_distance;
            lidar_value.data[1]=shortest_angle;
            command_publisher_->publish(lidar_value);
        }
        
    public:
    ObsDet(std::string name):Node(name)
    {
        RCLCPP_INFO(this->get_logger(), "大家好，我是%s.", name.c_str());
        // 订阅激光雷达主题
        laser_scan_sub_=this->create_subscription<sensor_msgs::msg::LaserScan>(
            "scan",10,std::bind(&ObsDet::lidar_callback,this,std::placeholders::_1));
        // 创建发布者
        command_publisher_=this->create_publisher<std_msgs::msg::Float32MultiArray>(
            "lidar_dis_angle", 10);
    }
};

int main(int argc, char **argv)
{
    rclcpp::init(argc, argv);
    /*创建对应节点的共享指针对象*/
    auto node = std::make_shared<ObsDet>("obs_det");
    /* 运行节点，并检测退出信号*/
    rclcpp::spin(node);
    rclcpp::shutdown();
    return 0;
}